#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
位置管理模块 - Position Manager Module

该模块负责管理机器人的位置信息，包括：
- 保存当前位置到配置文件
- 列出所有已保存的位置
- 获取指定名称的位置信息
- 删除已保存的位置
- 位置名称验证和配置文件管理

This module manages robot position information including:
- Save current position to config file
- List all saved positions
- Get position information by name
- Delete saved positions
- Position name validation and config file management
"""

import os
import json
import time
from typing import Optional, Union, Dict, Any


class PositionManager:
    """
    位置管理器类
    Position Manager Class
    
    负责管理机器人的位置保存、加载、删除等功能
    Manages robot position saving, loading, deletion and other functions
    """
    
    def __init__(self, muto_instance=None):
        """
        初始化位置管理器
        Initialize position manager
        
        Args:
            muto_instance: Muto机器人实例 Muto robot instance
        """
        self.muto = muto_instance
    
    # ==================== 配置文件管理 Config File Management ====================
    
    def _get_positions_config_path(self) -> str:
        """
        获取位置配置文件路径
        Get position config file path
        
        Returns:
            str: 配置文件完整路径 Full path to config file
        """
        home_dir = os.path.expanduser("~")
        map_dir = os.path.join(home_dir, "map")
        
        # 确保目录存在
        if not os.path.exists(map_dir):
            os.makedirs(map_dir, exist_ok=True)
            
        return os.path.join(map_dir, "saved_positions.json")
    
    def _load_saved_positions(self) -> dict:
        """
        加载已保存的位置配置
        Load saved position configurations
        
        Returns:
            dict: 位置配置字典 Position configuration dictionary
        """
        config_path = self._get_positions_config_path()
        
        if not os.path.exists(config_path):
            return {}
            
        try:
            with open(config_path, 'r', encoding='utf-8') as f:
                return json.load(f)
        except (json.JSONDecodeError, IOError) as e:
            if hasattr(self.muto, 'debug') and self.muto.debug:
                print(f"[DEBUG] 加载位置配置失败 Failed to load position config: {e}")
            return {}
    
    def _save_positions_config(self, positions: dict) -> bool:
        """
        保存位置配置到文件
        Save position configuration to file
        
        Args:
            positions: 位置配置字典 Position configuration dictionary
            
        Returns:
            bool: 保存是否成功 Whether save was successful
        """
        config_path = self._get_positions_config_path()
        
        try:
            with open(config_path, 'w', encoding='utf-8') as f:
                json.dump(positions, f, ensure_ascii=False, indent=2)
            return True
        except IOError as e:
            if hasattr(self.muto, 'debug') and self.muto.debug:
                print(f"[DEBUG] 保存位置配置失败 Failed to save position config: {e}")
            return False
    
    # ==================== 位置名称验证 Position Name Validation ====================
    
    def _validate_position_name(self, name: str) -> tuple:
        """
        验证位置名称的有效性
        Validate position name validity
        
        Args:
            name: 位置名称 Position name
            
        Returns:
            tuple: (is_valid: bool, validated_name_or_error: str)
                is_valid: 名称是否有效 Whether name is valid
                validated_name_or_error: 验证后的名称或错误信息 Validated name or error message
        """
        if not name or not isinstance(name, str):
            return False, "位置名称不能为空 Position name cannot be empty"
            
        name = name.strip()
        if not name:
            return False, "位置名称不能为空 Position name cannot be empty"
            
        if len(name) > 50:
            return False, "位置名称过长，最多50个字符 Position name too long, max 50 characters"
            
        # 检查是否包含非法字符
        invalid_chars = ['/', '\\', ':', '*', '?', '"', '<', '>', '|']
        for char in invalid_chars:
            if char in name:
                return False, f"位置名称包含非法字符: {char} Position name contains invalid character: {char}"
                
        return True, name.strip()
    
    # ==================== 位置管理功能 Position Management Functions ====================
    
    def save_current_position(self, position_name: str, timeout_s: float = 10.0) -> tuple:
        """
        保存当前位置
        Save current position
        
        Args:
            position_name: 位置名称 Position name
            timeout_s: 获取位置的超时时间(秒) Timeout for getting position in seconds
            
        Returns:
            tuple: (success: bool, message: str, data: dict)
                success: 是否成功 Whether successful
                message: 操作结果消息 Operation result message
                data: 保存的位置数据或空字典 Saved position data or empty dict
        
        Examples:
            success, msg, data = position_manager.save_current_position("living_room")
        """
        try:
            # 验证位置名称
            is_valid, validated_name = self._validate_position_name(position_name)
            if not is_valid:
                return False, validated_name, {}
            
            # 获取当前位置
            success, message, position_data = self.muto.sensor_manager.get_robot_position_in_map(timeout_s)
            if not success:
                return False, f"无法获取当前位置: {message} Cannot get current position: {message}", {}
            
            # 加载现有位置配置
            saved_positions = self._load_saved_positions()
            
            # 准备保存的位置数据
            save_data = {
                "position": position_data["position"],
                "orientation": position_data["orientation"],
                "saved_time": time.time(),
                "saved_time_str": time.strftime("%Y-%m-%d %H:%M:%S", time.localtime()),
                "frame_id": position_data.get("frame_id", "map"),
                "child_frame_id": position_data.get("child_frame_id", "base_link")
            }
            
            # 检查是否已存在同名位置
            position_exists = validated_name in saved_positions
            
            # 保存位置
            saved_positions[validated_name] = save_data
            
            # 写入配置文件
            if not self._save_positions_config(saved_positions):
                return False, "保存位置配置失败 Failed to save position config", {}
            
            if hasattr(self.muto, 'debug') and self.muto.debug:
                print(f"[DEBUG] 位置 '{validated_name}' 保存成功 Position '{validated_name}' saved successfully")
                print(f"[DEBUG] 位置坐标 Position coordinates: x={save_data['position']['x']:.3f}, y={save_data['position']['y']:.3f}, z={save_data['position']['z']:.3f}")
            
            action = "更新" if position_exists else "保存"
            action_en = "updated" if position_exists else "saved"
            return True, f"成功{action}位置 '{validated_name}' Successfully {action_en} position '{validated_name}'", save_data
            
        except Exception as e:
            if hasattr(self.muto, 'debug') and self.muto.debug:
                print(f"[DEBUG] 保存位置出错 Error saving position: {e}")
            return False, f"保存位置出错 Error saving position: {str(e)}", {}
    
    def list_saved_positions(self) -> tuple:
        """
        列出所有已保存的位置
        List all saved positions
        
        Returns:
            tuple: (success: bool, message: str, data: dict)
                success: 是否成功 Whether successful
                message: 操作结果消息 Operation result message
                data: 位置列表字典或空字典 Position list dictionary or empty dict
        
        Examples:
            success, msg, positions = position_manager.list_saved_positions()
        """
        try:
            saved_positions = self._load_saved_positions()
            
            if not saved_positions:
                return True, "没有已保存的位置 No saved positions", {}
            
            # 构建返回数据
            positions_info = {}
            for name, data in saved_positions.items():
                positions_info[name] = {
                    "position": data["position"],
                    "orientation": data["orientation"],
                    "saved_time": data.get("saved_time_str", "未知 Unknown"),
                    "frame_id": data.get("frame_id", "map")
                }
            
            if hasattr(self.muto, 'debug') and self.muto.debug:
                print(f"[DEBUG] 找到 {len(saved_positions)} 个已保存的位置 Found {len(saved_positions)} saved positions")
                for name in saved_positions.keys():
                    print(f"[DEBUG] - {name}")
            
            return True, f"找到 {len(saved_positions)} 个已保存的位置 Found {len(saved_positions)} saved positions", positions_info
            
        except Exception as e:
            if hasattr(self.muto, 'debug') and self.muto.debug:
                print(f"[DEBUG] 列出位置出错 Error listing positions: {e}")
            return False, f"列出位置出错 Error listing positions: {str(e)}", {}
    
    def get_saved_position(self, position_name: str) -> tuple:
        """
        获取指定名称的已保存位置
        Get saved position by name
        
        Args:
            position_name: 位置名称 Position name
            
        Returns:
            tuple: (success: bool, message: str, data: dict)
                success: 是否成功 Whether successful
                message: 操作结果消息 Operation result message
                data: 位置数据或空字典 Position data or empty dict
        
        Examples:
            success, msg, position = position_manager.get_saved_position("living_room")
        """
        try:
            # 验证位置名称
            is_valid, validated_name = self._validate_position_name(position_name)
            if not is_valid:
                return False, validated_name, {}
            
            saved_positions = self._load_saved_positions()
            
            if validated_name not in saved_positions:
                return False, f"位置 '{validated_name}' 不存在 Position '{validated_name}' does not exist", {}
            
            position_data = saved_positions[validated_name]
            
            if hasattr(self.muto, 'debug') and self.muto.debug:
                print(f"[DEBUG] 获取位置 '{validated_name}' 成功 Successfully retrieved position '{validated_name}'")
                print(f"[DEBUG] 位置坐标 Position coordinates: x={position_data['position']['x']:.3f}, y={position_data['position']['y']:.3f}, z={position_data['position']['z']:.3f}")
            
            return True, f"成功获取位置 '{validated_name}' Successfully retrieved position '{validated_name}'", position_data
            
        except Exception as e:
            if hasattr(self.muto, 'debug') and self.muto.debug:
                print(f"[DEBUG] 获取位置出错 Error getting position: {e}")
            return False, f"获取位置出错 Error getting position: {str(e)}", {}
    
    def delete_saved_position(self, position_name: str) -> tuple:
        """
        删除指定名称的已保存位置
        Delete saved position by name
        
        Args:
            position_name: 位置名称 Position name
            
        Returns:
            tuple: (success: bool, message: str, data: dict)
                success: 是否成功 Whether successful
                message: 操作结果消息 Operation result message
                data: 被删除的位置数据或空字典 Deleted position data or empty dict
        
        Examples:
            success, msg, deleted_data = position_manager.delete_saved_position("old_position")
        """
        try:
            # 验证位置名称
            is_valid, validated_name = self._validate_position_name(position_name)
            if not is_valid:
                return False, validated_name, {}
            
            saved_positions = self._load_saved_positions()
            
            if validated_name not in saved_positions:
                return False, f"位置 '{validated_name}' 不存在 Position '{validated_name}' does not exist", {}
            
            # 获取要删除的位置数据
            deleted_data = saved_positions[validated_name]
            
            # 删除位置
            del saved_positions[validated_name]
            
            # 保存更新后的配置
            if not self._save_positions_config(saved_positions):
                return False, "保存位置配置失败 Failed to save position config", {}
            
            if hasattr(self.muto, 'debug') and self.muto.debug:
                print(f"[DEBUG] 位置 '{validated_name}' 删除成功 Position '{validated_name}' deleted successfully")
            
            return True, f"成功删除位置 '{validated_name}' Successfully deleted position '{validated_name}'", deleted_data
            
        except Exception as e:
            if hasattr(self.muto, 'debug') and self.muto.debug:
                print(f"[DEBUG] 删除位置出错 Error deleting position: {e}")
            return False, f"删除位置出错 Error deleting position: {str(e)}", {}
    
    # ==================== 辅助功能 Helper Functions ====================
    
    def get_positions_count(self) -> tuple:
        """
        获取已保存位置的数量
        Get count of saved positions
        
        Returns:
            tuple: (success: bool, message: str, data: dict)
                success: 是否成功 Whether successful
                message: 操作结果消息 Operation result message
                data: 包含数量信息的字典 Dictionary containing count information
        """
        try:
            saved_positions = self._load_saved_positions()
            count = len(saved_positions)
            
            return True, f"共有 {count} 个已保存的位置 Total {count} saved positions", {
                "count": count,
                "positions": list(saved_positions.keys()) if saved_positions else []
            }
            
        except Exception as e:
            if hasattr(self.muto, 'debug') and self.muto.debug:
                print(f"[DEBUG] 获取位置数量出错 Error getting positions count: {e}")
            return False, f"获取位置数量出错 Error getting positions count: {str(e)}", {}
    
    def position_exists(self, position_name: str) -> tuple:
        """
        检查指定名称的位置是否存在
        Check if position with specified name exists
        
        Args:
            position_name: 位置名称 Position name
            
        Returns:
            tuple: (success: bool, message: str, data: dict)
                success: 检查是否成功 Whether check was successful
                message: 检查结果消息 Check result message
                data: 包含存在状态的字典 Dictionary containing existence status
        """
        try:
            # 验证位置名称
            is_valid, validated_name = self._validate_position_name(position_name)
            if not is_valid:
                return False, validated_name, {}
            
            saved_positions = self._load_saved_positions()
            exists = validated_name in saved_positions
            
            return True, f"位置 '{validated_name}' {'存在' if exists else '不存在'} Position '{validated_name}' {'exists' if exists else 'does not exist'}", {
                "position_name": validated_name,
                "exists": exists
            }
            
        except Exception as e:
            if hasattr(self.muto, 'debug') and self.muto.debug:
                print(f"[DEBUG] 检查位置存在性出错 Error checking position existence: {e}")
            return False, f"检查位置存在性出错 Error checking position existence: {str(e)}", {}
    
    def clear_all_positions(self, confirm: bool = False) -> tuple:
        """
        清空所有已保存的位置（危险操作）
        Clear all saved positions (dangerous operation)
        
        Args:
            confirm: 确认执行清空操作 Confirm to execute clear operation
            
        Returns:
            tuple: (success: bool, message: str, data: dict)
                success: 是否成功 Whether successful
                message: 操作结果消息 Operation result message
                data: 清空前的位置数据 Position data before clearing
        """
        try:
            if not confirm:
                return False, "需要确认参数才能执行清空操作 Confirmation parameter required to execute clear operation", {}
            
            # 加载现有位置（用于返回）
            saved_positions = self._load_saved_positions()
            positions_backup = saved_positions.copy()
            
            # 清空位置配置
            if not self._save_positions_config({}):
                return False, "清空位置配置失败 Failed to clear position config", {}
            
            if hasattr(self.muto, 'debug') and self.muto.debug:
                print(f"[DEBUG] 已清空 {len(positions_backup)} 个位置 Cleared {len(positions_backup)} positions")
            
            return True, f"成功清空 {len(positions_backup)} 个位置 Successfully cleared {len(positions_backup)} positions", {
                "cleared_count": len(positions_backup),
                "cleared_positions": positions_backup
            }
            
        except Exception as e:
            if hasattr(self.muto, 'debug') and self.muto.debug:
                print(f"[DEBUG] 清空位置出错 Error clearing positions: {e}")
            return False, f"清空位置出错 Error clearing positions: {str(e)}", {}